/*
  SOLID - Interference Detection Library
  Copyright (C) 1997 Gino van den Bergen

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Library General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Library General Public License for more details.

  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the Free
  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

  Please send remarks, questions and bug reports to gino@win.tue.nl,
  or write to:
                  Gino van den Bergen
		  Department of Mathematics and Computing Science
		  Eindhoven University of Technology
		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
*/

#include "BBox.h"
#include "Polygon.h"


BBox::BBox(int n, const Polygon **p) {
  Point min(INFINITY, INFINITY, INFINITY);
  Point max(-INFINITY, -INFINITY, -INFINITY);

  for (int i = 0; i != n; ++i) 
    for (int j = 0; j != p[i]->size(); ++j) {
      min.setInf((*p[i])[j]);
      max.setSup((*p[i])[j]);
    }
  min /= 2;
  max /= 2;
  center = min + max;
  extent = max - min;

	printf("COSTRUTTORE Box - inizio\n");
    printf("nmesh: %d\n",n);
    printf("min: %g\t%g\t%g\n",min[X],min[Y],min[Z]);
    printf("max: %g\t%g\t%g\n",max[X],max[Y],max[Z]);
    printf("center\textent:\n");
    printf("%g\t%g\t%g\t%g\t%g\t%g\n",center[X],center[Y],center[Z],extent[X],extent[Y],extent[Z]);
	printf("COSTRUTTORE Box - fine\n");
}

void BBox::setValue(const BBox& b, const Transform& t) {
  center = t(b.center);
  Matrix m = t.getOrientation().absolute();
  extent.setValue(m.rowdot(X, b.extent), 
		  m.rowdot(Y, b.extent), 
		  m.rowdot(Z, b.extent));
}

// From: S. Gottschalk, M. C. Lin and D. Manocha, "OBB-Tree: A Hierarchical
//       Structure for Rapid Interference Detection", Proc. SIGGRAPH '96, 
//       pg. 171-180 

bool sep_axis_test(const Vector& a, const Vector& b, const Vector& pos, 
		   const Matrix& orn, const Matrix& m) {
/*                 
	printf("sep_axis_test - inizio\n");
    printf("a: %g\t%g\t%g\n",a[0],a[1],a[2]);
    printf("b: %g\t%g\t%g\n",b[0],b[1],b[2]);
    printf("pos: %g\t%g\t%g\n",pos[0],pos[1],pos[2]);
    printf("orn: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",orn[0][0],orn[0][1],orn[0][2],orn[1][0],orn[1][1],orn[1][2],orn[2][0],orn[2][1],orn[2][2]);
    printf("m: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",m[0][0],m[0][1],m[0][2],m[1][0],m[1][1],m[1][2],m[2][0],m[2][1],m[2][2]);
	printf("sep_axis_test - fine\n");
*/
  if (a[X] + m.rowdot(X, b) < abs(pos[X])) return false;
  if (a[Y] + m.rowdot(Y, b) < abs(pos[Y])) return false;
  if (a[Z] + m.rowdot(Z, b) < abs(pos[Z])) return false;
  
  if (dot(a, m[X]) + b[X] < abs(dot(pos, orn[X]))) return false;  
  if (dot(a, m[Y]) + b[Y] < abs(dot(pos, orn[Y]))) return false;   
  if (dot(a, m[Z]) + b[Z] < abs(dot(pos, orn[Z]))) return false;  

  if (a[Y] * m[X][Z] + a[Z] * m[X][Y] + b[Y] * m[Z][X] + b[Z] * m[Y][X] <  
      abs(pos[Z] * orn[X][Y] - pos[Y] * orn[X][Z]))
    return false;
  
  if (a[Y] * m[Y][Z] + a[Z] * m[Y][Y] + b[X] * m[Z][X] + b[Z] * m[X][X] <
      abs(pos[Z] * orn[Y][Y] - pos[Y] * orn[Y][Z]))
    return false;
  
  if (a[Y] * m[Z][Z] + a[Z] * m[Z][Y] + b[X] * m[Y][X] + b[Y] * m[X][X] <
      abs(pos[Z] * orn[Z][Y] - pos[Y] * orn[Z][Z]))
    return false;
  
  if (a[X] * m[X][Z] + a[Z] * m[X][X] + b[Y] * m[Z][Y] + b[Z] * m[Y][Y] <
      abs(pos[X] * orn[X][Z] - pos[Z] * orn[X][X]))
    return false;
  
  if (a[X] * m[Y][Z] + a[Z] * m[Y][X] + b[X] * m[Z][Y] + b[Z] * m[X][Y] < 
      abs(pos[X] * orn[Y][Z] - pos[Z] * orn[Y][X]))
    return false;
  
  if (a[X] * m[Z][Z] + a[Z] * m[Z][X] + b[X] * m[Y][Y] + b[Y] * m[X][Y] <
      abs(pos[X] * orn[Z][Z] - pos[Z] * orn[Z][X]))
    return false;
  
  if (a[X] * m[X][Y] + a[Y] * m[X][X] + b[Y] * m[Z][Z] + b[Z] * m[Y][Z] <
      abs(pos[Y] * orn[X][X] - pos[X] * orn[X][Y]))
    return false;
  
  if (a[X] * m[Y][Y] + a[Y] * m[Y][X] + b[X] * m[Z][Z] + b[Z] * m[X][Z] <
      abs(pos[Y] * orn[Y][X] - pos[X] * orn[Y][Y]))
    return false;
  
  if (a[X] * m[Z][Y] + a[Y] * m[Z][X] + b[X] * m[Y][Z] + b[Y] * m[X][Z] <
      abs(pos[Y] * orn[Z][X] - pos[X] * orn[Z][Y]))     
    return false;
  
  return true;
}



